#include <stdio.h>
#include <string.h>
#include "oled_display.h"
#include "ssd1306.h"
#include "robot_control.h"

extern unsigned char g_left_motor_enable;
extern unsigned char g_right_motor_enable;
extern unsigned char g_left_motor_direction;
extern unsigned char g_right_motor_direction;

// OLED显示初始化
void oled_display_init(void)
{
    // 显示欢迎信息
    oled_display_welcome();
}

// 显示欢迎界面
void oled_display_welcome(void)
{
    ssd1306_Fill(Black);
    ssd1306_SetCursor(10, 10);
    ssd1306_DrawString("Robot Car", Font_7x10, White);
    ssd1306_SetCursor(5, 25);
    ssd1306_DrawString("Speed Control", Font_7x10, White);
    ssd1306_SetCursor(15, 40);
    ssd1306_DrawString("Ready!", Font_7x10, White);
    ssd1306_UpdateScreen();
}

// 显示小车状态和速度
void oled_display_car_status(unsigned char car_status, unsigned char speed_level)
{
    char status_str[32];
    char speed_str[32];
    char car_action_str[32];

    // 清空屏幕
    ssd1306_Fill(Black);

    // 显示车子当前动作状态（第一行）
    ssd1306_SetCursor(0, OLED_TITLE_Y);
    // 电机状态变量已在robot_control.h中声明

    // 判断车子当前状态
    if (!g_left_motor_enable && !g_right_motor_enable) {
        strcpy(car_action_str, "Stop");
    } else if (g_left_motor_enable && g_right_motor_enable) {
        if (g_left_motor_direction == 0 && g_right_motor_direction == 0) {
            strcpy(car_action_str, "Forward");
        } else if (g_left_motor_direction == 1 && g_right_motor_direction == 1) {
            strcpy(car_action_str, "Backward");
        } else {
            strcpy(car_action_str, "Turn");
        }
    } else if (g_left_motor_enable && !g_right_motor_enable) {
        strcpy(car_action_str, "Right");
    } else if (!g_left_motor_enable && g_right_motor_enable) {
        strcpy(car_action_str, "Left");
    } else {
        strcpy(car_action_str, "Unknown");
    }

    ssd1306_DrawString(car_action_str, Font_7x10, White);
    
    // 显示工作模式
    ssd1306_SetCursor(0, OLED_MODE_Y);
    switch(car_status) {
        case CAR_OBSTACLE_AVOIDANCE_STATUS:
            strcpy(status_str, "Mode: Obstacle");
            break;
        case CAR_STOP_STATUS:
            strcpy(status_str, "Mode: Stop");
            break;
        case CAR_TRACE_STATUS:
            strcpy(status_str, "Mode: Trace");
            break;
        default:
            strcpy(status_str, "Mode: Unknown");
            break;
    }
    ssd1306_DrawString(status_str, Font_7x10, White);
    
    // 显示速度信息
    ssd1306_SetCursor(0, OLED_SPEED_Y);
    snprintf(speed_str, sizeof(speed_str), "Speed: %d%%", speed_level);
    ssd1306_DrawString(speed_str, Font_7x10, White);
    
    // 显示速度等级条
    oled_display_speed_bar(speed_level);
    
    // 更新屏幕显示
    ssd1306_UpdateScreen();
}

// 显示速度等级条
void oled_display_speed_bar(unsigned char speed_level)
{
    // 显示"Level:"标签
    ssd1306_SetCursor(0, OLED_BAR_Y);
    ssd1306_DrawString("Level:", Font_7x10, White);

    // 计算当前等级 (0-100 -> 0-5)
    int current_level;
    if (speed_level == 0) {
        current_level = 0;  // 0%显示0个方块
    } else if (speed_level <= 30) {
        current_level = 1;  // 30%显示1个方块
    } else if (speed_level <= 50) {
        current_level = 2;  // 50%显示2个方块
    } else if (speed_level <= 70) {
        current_level = 3;  // 70%显示3个方块
    } else if (speed_level <= 90) {
        current_level = 4;  // 90%显示4个方块
    } else {
        current_level = 5;  // 100%显示5个方块（满格！）
    }

    // 绘制速度等级指示条
    for (int i = 0; i < SPEED_BAR_COUNT; i++) {
        int x = SPEED_BAR_X + i * (SPEED_BAR_WIDTH + SPEED_BAR_SPACING);
        int y = OLED_BAR_Y;

        if (i < current_level) {
            // 填充的方块表示当前等级
            ssd1306_DrawRectangle(x, y, x + SPEED_BAR_WIDTH, y + SPEED_BAR_HEIGHT, White);

            // 填充内部
            for (int fill_y = y + 1; fill_y < y + SPEED_BAR_HEIGHT; fill_y++) {
                for (int fill_x = x + 1; fill_x < x + SPEED_BAR_WIDTH; fill_x++) {
                    ssd1306_DrawPixel(fill_x, fill_y, White);
                }
            }
        } else {
            // 空心方块表示未达到的等级
            ssd1306_DrawRectangle(x, y, x + SPEED_BAR_WIDTH, y + SPEED_BAR_HEIGHT, White);
        }
    }

    // 在速度条右侧显示数字等级
    char level_str[8];
    snprintf(level_str, sizeof(level_str), "%d/5", current_level);
    ssd1306_SetCursor(SPEED_BAR_X + SPEED_BAR_COUNT * (SPEED_BAR_WIDTH + SPEED_BAR_SPACING) + 5, OLED_BAR_Y);
    ssd1306_DrawString(level_str, Font_7x10, White);
}

// 显示速度变化动画
void oled_display_speed_change(unsigned char old_speed, unsigned char new_speed)
{
    char speed_str[32];

    // 清空屏幕
    ssd1306_Fill(Black);

    // 显示速度变化信息
    ssd1306_SetCursor(10, 15);
    ssd1306_DrawString("Speed Changed!", Font_7x10, White);

    ssd1306_SetCursor(5, 30);
    snprintf(speed_str, sizeof(speed_str), "%d%% -> %d%%", old_speed, new_speed);
    ssd1306_DrawString(speed_str, Font_7x10, White);

    // 显示新的速度条
    oled_display_speed_bar(new_speed);

    ssd1306_UpdateScreen();
}

// 显示模式变化
void oled_display_mode_change(unsigned char new_mode)
{
    char mode_str[32];

    // 清空屏幕
    ssd1306_Fill(Black);

    // 显示模式变化信息
    ssd1306_SetCursor(10, 15);
    ssd1306_DrawString("Mode Changed!", Font_7x10, White);

    ssd1306_SetCursor(5, 30);
    switch(new_mode) {
        case CAR_OBSTACLE_AVOIDANCE_STATUS:
            strcpy(mode_str, "Obstacle Avoid");
            break;
        case CAR_STOP_STATUS:
            strcpy(mode_str, "Stop Mode");
            break;
        case CAR_TRACE_STATUS:
            strcpy(mode_str, "Trace Mode");
            break;
        default:
            strcpy(mode_str, "Unknown Mode");
            break;
    }
    ssd1306_DrawString(mode_str, Font_7x10, White);

    ssd1306_UpdateScreen();
}
